/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package Robo;

import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Encoder; 
/**
 *
 * @author paul
 */
public class RoboManipulator extends RoboTask {
    static double[] pegHeights = {12, 18, 20, 21, 22, 25, 30};

    static double maxHeight = 30;
    static double baseHeight = 12;
    static double ticksPerInch = 10;
    static double bufferHeight = 4;
    static double heightAccuracy = 0.5;

    static double ticksPerDegree = 10;
    static double bufferAngle = 5;
    static double angleAccuracy = 1;

    double currentHeight = baseHeight;
    double targetHeight = baseHeight;
    double currentElevatorPower = 0;
    boolean elevatorHeightKnown = false;

    double currentAngle = 0;
    double targetAngle = 0;
    double currentArmPower = 0;
    boolean armAngleKnown = false;

    private double lastSimTime = 0.0;

    boolean armMoving = false;
    boolean elevatorMoving = false;
    
    private boolean trace = false;
    private double elevSpeedScale = 6.0;   // max speed in/sec
    private double armSpeedScale = 30.0;   // max speed deg/sec

    Jaguar rollerbeta = new Jaguar(8);
    Jaguar rolleralpha = new Jaguar(7);
    Jaguar manipulator = new Jaguar(6);  // 1.0 power = 90RPM....0.5 power = 80 RPM....0.25 power = 48 RPM   NOTE: JAGUAR AND VICTOR POWER VALUES MAY BE DIFFERENT
    Jaguar elevator = new Jaguar(5);     // 1.0 power = 90RPM....0.5 power = 80 RPM....0.25 power = 48 RPM

    public Encoder elevatorEncoder = new Encoder(4, 5);
    public DigitalInput elevatorUp = new DigitalInput(1, 6);
    public Encoder armEncoder = new Encoder(7, 8);
    public DigitalInput armUp = new DigitalInput(1, 9);

    RoboManipulator(){
        elevator.setExpiration(15);
        elevator.set(0.0);
        manipulator.set(0.0);
        rolleralpha.set(0.0);
        rollerbeta.set(0.0);
    }

    void goToPeg(int number){
        moveElevatorTo(pegHeights[number]);
        moveArmTo((number != 0)?0:100);
    }

    void moveElevatorTo(double height){
        targetHeight = height;
        elevatorMoving = (Math.abs(currentHeight - targetHeight) >= heightAccuracy);
    }

    void moveArmTo(double angle){
        targetAngle = angle;
        armMoving = (Math.abs(currentAngle - targetAngle) >= angleAccuracy);
    }

    boolean isMoving(){
        return (armMoving || elevatorMoving);
    }

    public void handle(){
        super.handle();

        // reset encoders when top reached for the first time
        if(elevatorUp.get() && !elevatorHeightKnown ){
            elevatorEncoder.reset();
            elevatorHeightKnown = true;
        }
        if(armUp.get() && !armAngleKnown){
            armEncoder.reset();
            armAngleKnown = true;
        }

        // read height & angle

        // adjust elevator height
        double heightDifference = Math.abs(targetHeight - currentHeight);
        double direction = signum(targetHeight - currentHeight);
        if(heightDifference > bufferHeight) {
            // fast down
            moveElevator(direction);
        }
        else if(heightDifference > heightAccuracy) {
            // gradually slow down
            moveElevator(direction * ((heightDifference / bufferHeight) * 0.5 + 0.5));
        }
        else {
            // stop
            moveElevator(0);
        }

        // adjust arm angle
        double angleDifference = Math.abs(targetAngle - currentAngle);
        double armDirection = signum(targetAngle - currentAngle);
        if(angleDifference > bufferAngle) {
            // fast down
            moveArm(armDirection);
        }
        else if(angleDifference > angleAccuracy) {
            // gradually slow down
            moveArm(armDirection * ((angleDifference / bufferAngle) * 0.5 + 0.5));
        }
        else {
            // stop
            moveArm(0);
        }
    }
    void moveElevator(double power){
        if(power == currentElevatorPower) return;
        currentElevatorPower = power;
        elevatorMoving = (power != 0);
        // if(trace) System.out.printf("T=%1.2f: Elevator at %1.2f, moving %1.1f\n", taskTime, currentHeight, power);
    }
    void moveArm(double power){
        if(power == currentArmPower) return;
        currentArmPower = power;
        armMoving = (power != 0);
        // if(trace) System.out.printf("T=%1.2f: Arm at %1.2f, moving %1.1f\n", taskTime, currentAngle, power);
    }

    void simulate(){
        double dt = taskTime - lastSimTime;
        lastSimTime = taskTime;

        // simulate motion
        currentHeight  += currentElevatorPower * elevSpeedScale * dt;
        // simulate motion
        currentAngle  += currentArmPower * armSpeedScale * dt;
    }
}
